﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

namespace Happyfeet
{
    static class ImageUtil
    {
        public static int metric(int cell, int target)
        {
            int score = 0;
            score += Math.Abs(((cell >> 16) & 0xff) - ((target >> 16) & 0xff));
            score += Math.Abs(((cell >> 8) & 0xff) - ((target >> 8) & 0xff));
            score += Math.Abs(((cell) & 0xff) - ((target) & 0xff));
            return score;
        }

        public static ColorImagePoint findBestMatch(ColorImageStream img)
        {
            ColorImageFrame frame = img.OpenNextFrame(0);
            byte[] data = new byte[frame.PixelDataLength];
            frame.CopyPixelDataTo(data);
            int best = 0;
            int bestscore = 256 + 256 + 256; //Impossibly bad score
            int score = 0;
            for (int i = 0; i < frame.PixelDataLength; i += 3)
            {
                score = metric((int)(data[i] | data[i+1] << 8 | data[i+2] << 16), 0x0000FF);
                if (score < bestscore)
                {
                    bestscore = score;
                    best = i/3;
                }
            }
            ColorImagePoint cell = new ColorImagePoint();
            cell.X = best % frame.Width;
            cell.Y = best / frame.Width;
            return cell;
        }

        public static DepthImagePoint mapImageToDepthPixel(ColorImagePoint point)
        {
            //short depthValue = (short)(depthImage.Bits[depthIndex] | (depthImage.Bits[depthIndex + 1] << 8));
            return new DepthImagePoint();
        }
    }
}
